#include "board.h"
#include "Bujin.h"
#include <stdlib.h>
#include "Openmv.h"
#include "Control.h"
#include "bsp_mpu6050.h"
#include "Tracing.h"
#include "inv_mpu.h"
#include "Motor.h"
// 全局控制变量
int32_t L_PWM, R_PWM;
int Dir = 0;


int main(void) {
    SYSCFG_DL_init();

    DL_Timer_startCounter(PWM_0_INST);
    DL_Timer_startCounter(PWM_1_INST);
    NVIC_ClearPendingIRQ(TIMER_0_INST_INT_IRQN);//步进定时器清除
    NVIC_ClearPendingIRQ(TIMER_1_INST_INT_IRQN);//步进定时器清除
    NVIC_ClearPendingIRQ(OpenMV_INST_INT_IRQN); //OpenMV串口中断清除
    NVIC_EnableIRQ(TIMER_0_INST_INT_IRQN);
    NVIC_EnableIRQ(TIMER_1_INST_INT_IRQN);
    MPU6050_Init();
   // NVIC_EnableIRQ(OpenMV_INST_INT_IRQN);
    while (1) {
      //Set_Motor(-2444,500);
      lc_printf("State: 0x%02X, Error: %.2f\n\r", getTracingState(),Calculation_error());
      //delay_ms(100);
    }
}

// 映射参数定义
#define PWM_MAX     74999   // 停止状态值
#define PWM_MIN     50999   // 正转最快值
#define NEG_MIN    -10999   // 反转最快值
#define NEG_MAX    -54999   // 反转停止值
#define DIR_RANGE  100000   // Dir的标称范围
#define DEAD_ZONE  5        // 死区阈值

// 定时器的中断服务函数
void TIMER_1_INST_IRQHandler(void) {
    switch(DL_TimerG_getPendingInterrupt(TIMER_1_INST)) {
        case DL_TIMER_IIDX_ZERO: {
            // 获取原始控制量
             int rawDir = wh_Turn_Out(data1, 600, 400);
                        
             // 计算PWM值
             int pwmVal;
            if (rawDir >= 0) {
                 // 正转映射 [0, DIR_RANGE] -> [PWM_MIN, PWM_MAX]
                 float ratio = (float)rawDir / DIR_RANGE;
                 pwmVal = PWM_MAX - (int)(ratio * (PWM_MAX - PWM_MIN));
             } else {
                 // 反转映射 [-DIR_RANGE, 0] -> [NEG_MIN, NEG_MAX]
                 float ratio = (float)(-rawDir) / DIR_RANGE;
                 pwmVal = NEG_MAX + (int)(ratio * (NEG_MIN - NEG_MAX));
             }
            
            Set_PWM(-50999, 50999);
            break;
        }
        default:
            break;
    }
}